/*
 * Copyright (c) 2015-2016, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/***** Includes *****/
#include <xdc/std.h>
#include <xdc/runtime/System.h>
#include <xdc/runtime/Assert.h>

#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/knl/Clock.h>

/* Drivers */
#include <ti/drivers/rf/RF.h>
#include <ti/drivers/PIN.h>

/* Board Header files */
#include "Board.h"

#include <stdlib.h>
#include <wsnprotocol/WsnProtocol.h>
#include "easylink/EasyLink.h"

/***** Defines *****/
#define WSNPROTOCOL_TASK_STACK_SIZE 1024
#define WSNPROTOCOL_TASK_PRIORITY   3

#define WSNPROTOCOL_EVENT_ALL                   0xFFFFFFFF
#define WSNPROTOCOL_EVENT_TX_OPERATION           (uint32_t)(1 << 0)
#define WSNPROTOCOL_EVENT_VALID_PACKET_RECEIVED (uint32_t)(1 << 1)
#define WSNPROTOCOL_EVENT_RX_TIMEOUT            (uint32_t)(1 << 2)
#define WSNPROTOCOL_EVENT_RSP_SENT         (uint32_t)(1 << 3)
#define WSNPROTOCOL_EVENT_PACKET_ERROR         (uint32_t)(1 << 4)

#define OPERATION_NONE                    0
#define TX_OPERATION_PENDING              1
#define TX_OPERATION_INPROGRESS           2
#define RX_WAIT_OPERATION                 3
#define RX_PEND_OPERATION                 4

/***** Type declarations *****/
typedef struct  {
    EasyLink_TxPacket easyLinkTxPacket;
    uint8_t retriesDone;
    uint8_t maxNumberOfRetries;
    uint32_t ackTimeoutMs;
    uint8_t radioOperationState;
    uint8_t expectRspAddr;
    uint8_t expectRsp;
    WSNProtocol_Status_t result;
} RadioOperation_t;

typedef union {
    WSNProtocol_PacketHeader_t header;
    WSNProtocol_AckPacket_t ackPacket;
    WSNProtocol_AdcSensorPacket_t adcSensorPacket;
    WSNProtocol_DualModeSensorPacket_t dmSensorPacket;
    WSNProtocol_FwVersionReqPacket_t fwVersionReqPacket;
    WSNProtocol_FwVersionRspPacket_t fwVersionRspPacket;
    WSNProtocol_OadImgIdentifyReqPacket_t oadImgIdentifyReqPacket;
    WSNProtocol_OadImgIdentifyRspPacket_t oadImgIdentifyRspPacket;
    WSNProtocol_OadBlockReqPacket_t oadBlockReqPacket;
    WSNProtocol_OadBlockRspPacket_t oadBlockRspPacket;
} RadioProtocolPacket_t;

typedef void (*incomingPacketProcessFn_t)(RadioProtocolPacket_t packet);

typedef struct {
    uint8_t PacketType;
    incomingPacketProcessFn_t packetprocessFn;
}incomingPacketProcess_t;

static void processAck(RadioProtocolPacket_t packet);
static void processAdcPacket(RadioProtocolPacket_t packet);
static void processDmPacket(RadioProtocolPacket_t packet);
static void processFwVersioReq(RadioProtocolPacket_t packet);
static void processFwVersioRsp(RadioProtocolPacket_t packet);
static void processOadImgIdentifyReq(RadioProtocolPacket_t packet);
static void processOadImgIdentifyRsp(RadioProtocolPacket_t packet);
static void processOadImgBlockReq(RadioProtocolPacket_t packet);
static void processOadImgBlockRsp(RadioProtocolPacket_t packet);

static incomingPacketProcess_t incomingPacketProcessTable[] =
{
    {WSNPROTOCOL_PACKET_TYPE_ACK_PACKET,           processAck},
    {WSNPROTOCOL_PACKET_TYPE_ADC_SENSOR_PACKET,    processAdcPacket},
    {WSNPROTOCOL_PACKET_TYPE_DM_SENSOR_PACKET,     processDmPacket},
    {WSNPROTOCOL_PACKET_TYPE_FW_VERSION_REQ,       processFwVersioReq},
    {WSNPROTOCOL_PACKET_TYPE_FW_VERSION_RSP,       processFwVersioRsp},
    {WSNPROTOCOL_PACKET_TYPE_OAD_IMG_IDENTIFY_REQ, processOadImgIdentifyReq},
    {WSNPROTOCOL_PACKET_TYPE_OAD_IMG_IDENTIFY_RSP, processOadImgIdentifyRsp},
    {WSNPROTOCOL_PACKET_TYPE_OAD_BLOCK_REQ,        processOadImgBlockReq},
    {WSNPROTOCOL_PACKET_TYPE_OAD_BLOCK_RSP,        processOadImgBlockRsp},
};

static RadioProtocolPacket_t latestRxPacket;
static int8_t latestRxRssi;

/***** Variable declarations *****/
static Task_Params nodeRadioTaskParams;
Task_Struct nodeRadioTask;        /* not static so you can see in ROV */
static uint8_t nodeRadioTaskStack[WSNPROTOCOL_TASK_STACK_SIZE];
Semaphore_Struct radioInitSem;  /* not static so you can see in ROV */
static Semaphore_Handle radioInitHandle;
Event_Struct radioOperationEvent; /* not static so you can see in ROV */
static Event_Handle radioOperationEventHandle;
/*static*/ RadioOperation_t currentRadioOperation = {0};

/* Set Default parameters structure */
static const WSNProtocol_Params_t WSNProtocol_defaultParams = {
    .devType             = WSNProtocol_DevType_Concentrator,
    .devAddress          = WSNPROTOCOL_CONCENTRATOR_ADDRESS,
    .sleepy              = FALSE,
    .ackTimeoutMs        = WSNPROTOCOL_DEFAULT_ACK_TIMEOUT_TIME_MS,
    .rspTimeoutMs        = WSNPROTOCOL_DEFAULT_REQ_TIMEOUT_TIME_MS,
    .msgPendingTimeoutMs = WSNPROTOCOL_DEFAULT_MSG_PENDING_TIMEOUT_TIME_MS,
    .maxRetires          = WSNPROTOCOL_DEFAULT_MAX_RETRIES,
    .pCallbacks          = {0},
};

static WSNProtocol_Params_t WSNProtocol_params;

static Task_Handle taskSelf;

//TODO: change to mutex
bool inRx = false;

/***** Prototypes *****/
static void WSNProtocol_TaskFunction(UArg arg0, UArg arg1);
static WSNProtocol_Status_t prepTxOperation(uint8_t dstAddr, RadioProtocolPacket_t packet, uint8_t packetSize);
static WSNProtocol_Status_t sendTxRsp(uint8_t dstAddr, RadioProtocolPacket_t packet, uint8_t packetSize);
static void postTxOperation(void);
static void sendTxOperation(void);
static void sendTx(EasyLink_TxPacket txPacket);
static void rxDoneCallback(EasyLink_RxPacket * rxPacket, EasyLink_Status status);

/***** Function definitions *****/
void WSNProtocol_init(void) {

    /* Create semaphore used for blocking radio access requests */
    Semaphore_Params semParam;
    Semaphore_Params_init(&semParam);
    Semaphore_construct(&radioInitSem, 0, &semParam);
    radioInitHandle = Semaphore_handle(&radioInitSem);

    /* Create event used internally for state changes */
    Event_Params eventParam;
    Event_Params_init(&eventParam);
    Event_construct(&radioOperationEvent, &eventParam);
    radioOperationEventHandle = Event_handle(&radioOperationEvent);

    /* Create the wsn protocol task */
    Task_Params_init(&nodeRadioTaskParams);
    nodeRadioTaskParams.stackSize = WSNPROTOCOL_TASK_STACK_SIZE;
    nodeRadioTaskParams.priority = WSNPROTOCOL_TASK_PRIORITY;
    nodeRadioTaskParams.stack = &nodeRadioTaskStack;
    Task_construct(&nodeRadioTask, WSNProtocol_TaskFunction, &nodeRadioTaskParams, NULL);

    taskSelf = Task_handle(&nodeRadioTask);
}

void WSNProtocol_Params_init(WSNProtocol_Params_t *params)
{
    //Assert
    Assert_isTrue((params != NULL), NULL);

    *params = WSNProtocol_defaultParams;
}

void WSNProtocol_open(WSNProtocol_Params_t *params)
{
    // Populate default params if not provided
    if (params == NULL)
    {
        WSNProtocol_Params_init(&WSNProtocol_params);
        params = &WSNProtocol_params;
    }
    else
    {
        memcpy(&WSNProtocol_params, params, sizeof(WSNProtocol_Params_t));
    }

    /* Initialize EasyLink */
    if(EasyLink_init(WSNPROTOCOL_EASYLINK_MODULATION) != EasyLink_Status_Success) {
        System_abort("EasyLink_init failed");
    }

    /* If you wish to use a frequency other than the default use
     * the below API
     * EasyLink_setFrequency(868000000);
     */

    /* Set the filter to the generated random address */
    if (EasyLink_enableRxAddrFilter(&WSNProtocol_params.devAddress, 1, 1) != EasyLink_Status_Success)
    {
        System_abort("EasyLink_enableRxAddrFilter failed");
    }

    //Set operation to NONE
    currentRadioOperation.radioOperationState = OPERATION_NONE;

    //Allow app task to access radio
    Semaphore_post(radioInitHandle);
}

static void WSNProtocol_TaskFunction(UArg arg0, UArg arg1)
{
    /* Wait for radio to be configured */
    Semaphore_pend(radioInitHandle, BIOS_WAIT_FOREVER);

    /* Enter main task loop */
    while (1)
    {
        /* if concentrator OR if it is a node with radio opertion and then go back to Rx
         * ELSE if node with no radio operation et radio power down
         */
        if ( ((WSNProtocol_params.devType == WSNProtocol_DevType_Concentrator) &&
               (currentRadioOperation.radioOperationState != TX_OPERATION_INPROGRESS)   ) ||
             ((WSNProtocol_params.devType == WSNProtocol_DevType_Node) &&
               ((currentRadioOperation.radioOperationState == RX_WAIT_OPERATION) ||
                (currentRadioOperation.radioOperationState == RX_PEND_OPERATION))) )
        {
            if(EasyLink_receiveAsync(rxDoneCallback, 0) != EasyLink_Status_Success) {
                System_abort("EasyLink_receiveAsync failed");
            }
            inRx = true;
        }

        /* Wait for an event */
        uint32_t events = Event_pend(radioOperationEventHandle, 0, WSNPROTOCOL_EVENT_ALL, BIOS_WAIT_FOREVER);

        /* If we get an ACK timeout */
        if (events & WSNPROTOCOL_EVENT_TX_OPERATION)
        {
            if ((currentRadioOperation.radioOperationState == TX_OPERATION_PENDING) ||
                 (currentRadioOperation.radioOperationState == TX_OPERATION_INPROGRESS) )
            {
                sendTxOperation();
            }
        }

        if (events & WSNPROTOCOL_EVENT_VALID_PACKET_RECEIVED)
        {
            uint8_t cbIdx;

            /* Check if Ack is needed */
            if (WSNProtocol_isAsync(latestRxPacket.header.packetType))
            {
                bool messagePending = false;

                //Check if there is a message pending for this node
                if ((latestRxPacket.header.sourceAddress == currentRadioOperation.easyLinkTxPacket.dstAddr[0]) &&
                    (currentRadioOperation.radioOperationState == TX_OPERATION_PENDING) )
                {
                    //Mark message pending as true to keep the node awake
                    messagePending = true;
                    //Schedule the message to be Tx'ed
                    currentRadioOperation.radioOperationState = TX_OPERATION_INPROGRESS;
                    postTxOperation();
                }

                // Send the Ack
                WSNProtocol_sendAck(latestRxPacket.header.sourceAddress, messagePending);
            }

            /* If we got an Ack or Rsp that matches the expected, the src address matches
             * and we are in Tx operation, end the operation
             */
            if ((currentRadioOperation.radioOperationState == RX_PEND_OPERATION) ||
                ((currentRadioOperation.radioOperationState == RX_WAIT_OPERATION) &&
                 (currentRadioOperation.expectRspAddr == latestRxPacket.header.sourceAddress) &&
                 (currentRadioOperation.expectRsp == latestRxPacket.header.packetType)) )
            {
                currentRadioOperation.radioOperationState = OPERATION_NONE;

                /* reset timeout back to forever (0) */
                EasyLink_setCtrl(EasyLink_Ctrl_AsyncRx_TimeOut, 0);
            }

            /* Process the message */
            for(cbIdx = 0; cbIdx < (sizeof(incomingPacketProcessTable) / sizeof(incomingPacketProcess_t)) ;cbIdx++)
            {
                if(incomingPacketProcessTable[cbIdx].PacketType == latestRxPacket.header.packetType)
                {
                    incomingPacketProcessTable[cbIdx].packetprocessFn(latestRxPacket);
                }
            }
        }

        //Events do not need handling, but will wake the task and go back to Rx before pending on new event
        if (events & WSNPROTOCOL_EVENT_RX_TIMEOUT)
        {
            if(currentRadioOperation.radioOperationState == RX_WAIT_OPERATION)
            {
                /* resend packet */
                sendTxOperation();
            }
        }

        if (events & WSNPROTOCOL_EVENT_RSP_SENT)
        {
        }

        if (events & WSNPROTOCOL_EVENT_PACKET_ERROR)
        {
        }
    }
}

int8_t WSNProtocol_getLastRssi(void)
{
    return latestRxRssi;
}

bool WSNProtocol_isPending(void)
{
    bool isPending = false;

    if (currentRadioOperation.radioOperationState != OPERATION_NONE)
    {
        isPending = true;
    }

    return isPending;
}

void WSNProtocol_cancelPending(void)
{
    if (currentRadioOperation.radioOperationState != OPERATION_NONE)
    {
        currentRadioOperation.radioOperationState = OPERATION_NONE;

        //call application error callback
        if(WSNProtocol_params.pCallbacks->pfnMsgErrCb != NULL)
        {
            WSNProtocol_params.pCallbacks->pfnMsgErrCb(currentRadioOperation.expectRspAddr,
                                                        currentRadioOperation.expectRsp,
                                                        WSNProtocol_FailedCanceled);
        }
    }
}



WSNProtocol_Status_t WSNProtocol_sendAck(uint8_t dstAddress, bool messagePending)
{
    WSNProtocol_Status_t status;
    WSNProtocol_AckPacket_t ackPacket = {0};

    ackPacket.header.packetType = WSNPROTOCOL_PACKET_TYPE_ACK_PACKET;

    /* set / clear message pending bit */
    ackPacket.msgPending = messagePending;

    status = sendTxRsp(dstAddress, (RadioProtocolPacket_t) ackPacket, sizeof(WSNProtocol_AckPacket_t));

    return status;
}

WSNProtocol_Status_t WSNProtocol_sendDmPacket(uint8_t dstAddress, WSNProtocol_DmSensorData_t dmSensorData)
{
    WSNProtocol_Status_t status;
    WSNProtocol_DualModeSensorPacket_t dmSensorPacket;

    /* Setup ADC sensor packet */
    dmSensorPacket.header.packetType = WSNPROTOCOL_PACKET_TYPE_DM_SENSOR_PACKET;
    memcpy(&dmSensorPacket.dmSensorData, &dmSensorData, sizeof(WSNProtocol_DmSensorData_t));

    status = prepTxOperation(WSNPROTOCOL_CONCENTRATOR_ADDRESS, (RadioProtocolPacket_t) dmSensorPacket, sizeof(WSNProtocol_DualModeSensorPacket_t));

    return status;
}

WSNProtocol_Status_t WSNProtocol_sendFwVersionReq(uint8_t dstAddress)
{
    WSNProtocol_Status_t status;
    WSNProtocol_FwVersionReqPacket_t fwVersionReqPacket = {0};

    fwVersionReqPacket.header.packetType = WSNPROTOCOL_PACKET_TYPE_FW_VERSION_REQ;

    status = prepTxOperation(dstAddress, (RadioProtocolPacket_t) fwVersionReqPacket, sizeof(WSNProtocol_FwVersionReqPacket_t));

    return status;
}

WSNProtocol_Status_t WSNProtocol_sendFwVersionRsp(uint8_t dstAddress, char *fwVersion)
{
    WSNProtocol_Status_t status;
    WSNProtocol_FwVersionRspPacket_t fwVersionRspPacket = {0};

    fwVersionRspPacket.header.packetType = WSNPROTOCOL_PACKET_TYPE_FW_VERSION_RSP;
    memcpy(fwVersionRspPacket.fwVersion, fwVersion, WSNPROTOCOL_FW_VERSION_STR_LEN);

    status = sendTxRsp(dstAddress, (RadioProtocolPacket_t) fwVersionRspPacket, sizeof(WSNProtocol_FwVersionRspPacket_t));

    return status;
}

WSNProtocol_Status_t WSNProtocol_sendImgIdentifyReq(uint8_t dstAddress, uint8_t *pImgInfoData)
{
    WSNProtocol_Status_t status;
    WSNProtocol_OadImgIdentifyReqPacket_t oadImgIdentifyReqPacket = {0};

    oadImgIdentifyReqPacket.header.packetType = WSNPROTOCOL_PACKET_TYPE_OAD_IMG_IDENTIFY_REQ;
    memcpy(oadImgIdentifyReqPacket.imgMetaData, pImgInfoData, 16);

    status = prepTxOperation(dstAddress, (RadioProtocolPacket_t) oadImgIdentifyReqPacket, sizeof(WSNProtocol_OadImgIdentifyReqPacket_t));

    return status;
}

WSNProtocol_Status_t WSNProtocol_sendOadIdentifyImgRsp(uint8_t dstAddress, uint8_t rspStatus)
{
    WSNProtocol_OadImgIdentifyRspPacket_t oadImgIdentifyRspPacket = {0};
    WSNProtocol_Status_t status;

    oadImgIdentifyRspPacket.header.packetType = WSNPROTOCOL_PACKET_TYPE_OAD_IMG_IDENTIFY_RSP;
    oadImgIdentifyRspPacket.status = rspStatus;

    status = sendTxRsp(dstAddress, (RadioProtocolPacket_t) oadImgIdentifyRspPacket, sizeof(WSNProtocol_OadImgIdentifyRspPacket_t));

    return status;
}

WSNProtocol_Status_t WSNProtocol_sendOadImgBlockReq(uint8_t dstAddress, uint16_t blockNum)
{

    WSNProtocol_Status_t status;
    WSNProtocol_OadBlockReqPacket_t oadBlockReqPacket = {0};

    oadBlockReqPacket.header.packetType = WSNPROTOCOL_PACKET_TYPE_OAD_BLOCK_REQ;
    oadBlockReqPacket.blockNum = blockNum;

    status = prepTxOperation(dstAddress, (RadioProtocolPacket_t) oadBlockReqPacket, sizeof(WSNProtocol_OadBlockReqPacket_t));

    return status;
}

WSNProtocol_Status_t WSNProtocol_sendOadImgBlockRsp(uint8_t dstAddress, uint8_t *block)
{

    WSNProtocol_Status_t status;
    WSNProtocol_OadBlockRspPacket_t oadBlockRspPacket = {0};

    oadBlockRspPacket.header.packetType = WSNPROTOCOL_PACKET_TYPE_OAD_BLOCK_RSP;
    memcpy(oadBlockRspPacket.block, block, OAD_BLOCK_SIZE + 2);

    status = sendTxRsp(dstAddress, (RadioProtocolPacket_t) oadBlockRspPacket, sizeof(WSNProtocol_OadBlockRspPacket_t));

    return status;
}

static void processAck(RadioProtocolPacket_t packet)
{
    currentRadioOperation.radioOperationState = OPERATION_NONE;

    if (packet.ackPacket.msgPending)
    {
        currentRadioOperation.radioOperationState = RX_PEND_OPERATION;

        //set message pending time out
        EasyLink_setCtrl(EasyLink_Ctrl_AsyncRx_TimeOut, EasyLink_ms_To_RadioTime(WSNProtocol_params.msgPendingTimeoutMs));
        currentRadioOperation.maxNumberOfRetries = 0;

        //Rx will entered at the end of the function processing loop
    }

    //call application callback
    if(WSNProtocol_params.pCallbacks->pfnAckCb != NULL)
    {
        WSNProtocol_params.pCallbacks->pfnAckCb(packet.header.sourceAddress);
    }

}

static void processAdcPacket(RadioProtocolPacket_t packet)
{
    //call application callback
    if(WSNProtocol_params.pCallbacks->pfnDmPacketCb != NULL)
    {
        WSNProtocol_params.pCallbacks->pfnAdcPacketCb(packet.header.sourceAddress, packet.adcSensorPacket.adcValue);
    }
}

static void processDmPacket(RadioProtocolPacket_t packet)
{
    //call application callback
    if(WSNProtocol_params.pCallbacks->pfnDmPacketCb != NULL)
    {
        WSNProtocol_DmSensorData_t data;
        memcpy(&data, &packet.dmSensorPacket.dmSensorData, sizeof(WSNProtocol_DmSensorData_t));

        WSNProtocol_params.pCallbacks->pfnDmPacketCb(packet.header.sourceAddress, data);
    }
}

static void processFwVersioReq(RadioProtocolPacket_t packet)
{
    //call application callback
    if(WSNProtocol_params.pCallbacks->pfnFwVersionReqCb != NULL)
    {
        WSNProtocol_params.pCallbacks->pfnFwVersionReqCb(packet.header.sourceAddress);
    }
}

static void processFwVersioRsp(RadioProtocolPacket_t packet)
{
    //call application callback
    if(WSNProtocol_params.pCallbacks->pfnFwVersionRspCb != NULL)
    {
        WSNProtocol_params.pCallbacks->pfnFwVersionRspCb(packet.header.sourceAddress, packet.fwVersionRspPacket.fwVersion);
    }
}

static void processOadImgIdentifyReq(RadioProtocolPacket_t packet)
{
    //call application callback
    if(WSNProtocol_params.pCallbacks->pfnOadImgIdentifyReqCb != NULL)
    {
        WSNProtocol_params.pCallbacks->pfnOadImgIdentifyReqCb(packet.header.sourceAddress, packet.oadImgIdentifyReqPacket.imgMetaData);
    }
}

static void processOadImgIdentifyRsp(RadioProtocolPacket_t packet)
{
    //call application callback
    if(WSNProtocol_params.pCallbacks->pfnOadImgIdentifyRspCb != NULL)
    {
        WSNProtocol_params.pCallbacks->pfnOadImgIdentifyRspCb(packet.header.sourceAddress, packet.oadImgIdentifyRspPacket.status);
    }
}

static void processOadImgBlockReq(RadioProtocolPacket_t packet)
{
    //call application callback
    if(WSNProtocol_params.pCallbacks->pfnOadBlockReqCb != NULL)
    {
        WSNProtocol_params.pCallbacks->pfnOadBlockReqCb(packet.header.sourceAddress, packet.oadBlockReqPacket.blockNum);
    }
}

static void processOadImgBlockRsp(RadioProtocolPacket_t packet)
{
    //TODO: check this is resonse for req in progress
    if(currentRadioOperation.radioOperationState == TX_OPERATION_INPROGRESS)
    {
        currentRadioOperation.radioOperationState = OPERATION_NONE;
    }

    //call application callback
    if(WSNProtocol_params.pCallbacks->pfnOadBlockRspCb != NULL)
    {
        //TODO: Add block number to packet so application can check it is correct block
        WSNProtocol_params.pCallbacks->pfnOadBlockRspCb(packet.header.sourceAddress, packet.oadBlockRspPacket.block);
    }
}

static WSNProtocol_Status_t sendTxRsp(uint8_t dstAddress, RadioProtocolPacket_t packet, uint8_t packetSize)
{
    WSNProtocol_Status_t status = WSNProtocol_Failed;

    //TODO: Check if caller is in is not in this thread. Call prepTxOperation? Or return error?

    // check it is an rsp or ack.
    if ( WSNProtocol_isAck(packet.header.packetType) || WSNProtocol_isRsp(packet.header.packetType) )
    {
        //Prepare the txPacket
        EasyLink_TxPacket txPacket = {0};

        /* set dst address */
        txPacket.dstAddr[0] = dstAddress;

        /* Set source address in packt header */
        packet.header.sourceAddress = WSNProtocol_params.devAddress;

        /* Copy packet to payload
         * Note that the EasyLink API will implcitily both add the length byte and the destination address byte. */
        memcpy(txPacket.payload, ((uint8_t*)&packet), packetSize);
        txPacket.len = packetSize;

        sendTx(txPacket);

        //Was this an rsp call from another task
        if ((WSNProtocol_isRsp(packet.header.packetType)) && (
                taskSelf != Task_self()))
        {
            //TODO: should not be here, rsp must come from this thread!!!!
            while(1);
            /* This was a rsp, called from other task, hence the task processing loop will
             * nt re-enter Rx. Send WsnProtocol Task an event so it re-enters Rx */
            Event_post(radioOperationEventHandle, WSNPROTOCOL_EVENT_RSP_SENT);
        }

        status = WSNProtocol_Status_Success;
    }

    return status;
}

static WSNProtocol_Status_t prepTxOperation(uint8_t dstAddress, RadioProtocolPacket_t packet, uint8_t packetSize)
{
    WSNProtocol_Status_t status = WSNProtocol_FailedMsgInProgress;

    if (currentRadioOperation.radioOperationState == OPERATION_NONE)
    {
        /* Set source address in packt header */
        packet.header.sourceAddress = WSNProtocol_params.devAddress;

        /* Set destination address in EasyLink API */
        currentRadioOperation.easyLinkTxPacket.dstAddr[0] = dstAddress;

        /* Copy packet to payload
         * Note that the EasyLink API will implicitly both add the length byte and the destination address byte. */
        memcpy(currentRadioOperation.easyLinkTxPacket.payload, ((uint8_t*)&packet), packetSize);
        currentRadioOperation.easyLinkTxPacket.len = packetSize;

        /* Setup retries for req*/
        currentRadioOperation.maxNumberOfRetries = WSNProtocol_params.maxRetires;
        currentRadioOperation.ackTimeoutMs = WSNProtocol_params.rspTimeoutMs;
        currentRadioOperation.retriesDone = 0;

        /* Node will send to concentrator now as concentrator is always in Rx,
         * Concentrator will wait for incoming message and set the msg pending
         * bit in Ack.
         */
        if(WSNProtocol_params.devType == WSNProtocol_DevType_Node)
        {
            currentRadioOperation.radioOperationState = TX_OPERATION_INPROGRESS;
            postTxOperation();
        }
        else
        {
            currentRadioOperation.radioOperationState = TX_OPERATION_PENDING;
        }

        //TODO: start timer to cancel req incase node never wakes

        status = WSNProtocol_Status_Success;
    }
    else
    {
        status = WSNProtocol_FailedMsgInProgress;
    }

    return status;
}

static void postTxOperation(void)
{
    Event_post(radioOperationEventHandle, WSNPROTOCOL_EVENT_TX_OPERATION);
}

static void sendTxOperation(void)
{
    if (currentRadioOperation.retriesDone++ <= currentRadioOperation.maxNumberOfRetries)
    {
        /*  Allow the device sending request to enter Rx */
        currentRadioOperation.easyLinkTxPacket.absTime = EasyLink_getAbsTime() + EasyLink_ms_To_RadioTime(2);


        if( (WSNProtocol_isAsync(currentRadioOperation.easyLinkTxPacket.payload[PKT_TYPE_OFFSET])) ||
            (WSNProtocol_isReq(currentRadioOperation.easyLinkTxPacket.payload[PKT_TYPE_OFFSET])) )
        {
            /* set a Rx timeout for the Ack/Rsp */
            EasyLink_setCtrl(EasyLink_Ctrl_AsyncRx_TimeOut, EasyLink_ms_To_RadioTime(currentRadioOperation.ackTimeoutMs));

            currentRadioOperation.expectRspAddr = currentRadioOperation.easyLinkTxPacket.dstAddr[0];
            if(WSNProtocol_isAsync(currentRadioOperation.easyLinkTxPacket.payload[PKT_TYPE_OFFSET]))
            {
                currentRadioOperation.expectRsp = WSNPROTOCOL_PACKET_TYPE_ACK_PACKET;
            }
            else
            {
                currentRadioOperation.expectRsp =
                        currentRadioOperation.easyLinkTxPacket.payload[PKT_TYPE_OFFSET] + 1;
            }

            currentRadioOperation.radioOperationState = RX_WAIT_OPERATION;
        }

        /* Send packet  */
        sendTx(currentRadioOperation.easyLinkTxPacket);

        //Rx will entered at the end of the function processing loop
    }
    else
    {
        /* max retries, terminate currentRadioOperation */
        currentRadioOperation.radioOperationState = OPERATION_NONE;

        /* reset timeout back to forever (0) */
        EasyLink_setCtrl(EasyLink_Ctrl_AsyncRx_TimeOut, 0);

        //call application error callback
        if(WSNProtocol_params.pCallbacks->pfnMsgErrCb != NULL)
        {
            WSNProtocol_params.pCallbacks->pfnMsgErrCb(currentRadioOperation.expectRspAddr,
                                                        currentRadioOperation.expectRsp,
                                                        WSNProtocol_FailedTimeout);
        }
    }
}

static void sendTx(EasyLink_TxPacket txPacket)
{
    if (inRx)
    {
        /* abort the Rx */
        EasyLink_abort();

        //TODO: save Rx timeout and current time
    }

    /* Send packet  */
    if (EasyLink_transmit(&txPacket) != EasyLink_Status_Success)
    {
        System_abort("EasyLink_transmit failed");
    }

    //TODO: set new timeout for Rx
}

static void rxDoneCallback(EasyLink_RxPacket * rxPacket, EasyLink_Status status)
{
    inRx = false;

    /* If this callback is called because of a packet received */
    if (status == EasyLink_Status_Success)
    {
        /* Save packet */
        memcpy((void*)&latestRxPacket, &rxPacket->payload, rxPacket->len);
        latestRxRssi = rxPacket->rssi;

        /* Signal incoming packet received */
        Event_post(radioOperationEventHandle, WSNPROTOCOL_EVENT_VALID_PACKET_RECEIVED);
    }
    /* did the Rx timeout */
    else if(status == EasyLink_Status_Rx_Timeout)
    {
        Event_post(radioOperationEventHandle, WSNPROTOCOL_EVENT_RX_TIMEOUT);
    }
    else if(status == EasyLink_Status_Aborted)
    {
        /* do nothing, application aborted Rx to Tx */
    }
    else
    {
        /* Rx Error */
        Event_post(radioOperationEventHandle, WSNPROTOCOL_EVENT_PACKET_ERROR);
    }
}
